/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Implementation of TaskCtrl interface Server.
 * \author Antti Maula <antti.maula@tkk.fi>
 *
 * $Id: TaskCtrlServer.cpp,v 1.14 2009-10-19 07:59:42 mmatusia Exp $
 *
 */

#include "TaskCtrlServer.hpp"
#include "TaskCtrlData.hpp"

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
using namespace MaCI::TaskCtrl;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CTaskCtrlServer::CTaskCtrlServer(MaCICtrl::CMaCICtrlServer *aMaCICtrlServerPtr, 
                                 const int aInterfaceMinor,
                                 const std::string &aDescription,
                                 const std::string &aInstanceName)
  : CMaCI(aMaCICtrlServerPtr, GIMI_PROTOCOL_MACI_TASKCTRL, 
          aInterfaceMinor, aInstanceName, true),
    CSync(1,1),
    CThread(1),
    iCommandArray(),
    iServerActive(false),
    iServiceDescription(aDescription),
    iTaskCtrlSequenceNumber(0),
    iNextMajor(0)
{
}
//*****************************************************************************

CTaskCtrlServer::~CTaskCtrlServer()
{
  DoClose();
}
//*****************************************************************************

MaCI::EMaCIError CTaskCtrlServer::DoOpen(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);
  
  // Execute only if server is not active.
  if (!iServerActive) {
    // Register service.
    RegisterProvidedService("MaCI-TaskCtrlServer; '" + iServiceDescription + "'");
    RegisterAcceptedService("MaCI-TaskCtrlServer-Cmd; '" + iServiceDescription + "'");
    
    // Start the handler Thread
    SetActive(true);
    iServerActive = true;
    CThread::RunThread();
    
    result = KMaCIOK;
    
    dPrint(4,"Succesfully Open():ed TaskCtrlServer instance %p.", this);
  }
    
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CTaskCtrlServer::DoClose(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);

  // Execute block only if server is currently active.
  if (iServerActive) {
    // Stop!
    iServerActive = false;
    SetActive(false);
    // Stop the handler Thread
    /* CThread::CancelThread(); //Removed by script */
    CThread::WaitThread();    

    // Unregister services
    UnregisterProvidedService();
    UnregisterAcceptedService();
    
    dPrint(4,"Succesfully Close():ed TaskCtrlServer instance %p", this);
  }
  
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CTaskCtrlServer::SendTaskCtrlStatusEvent(const CTaskCtrlData &aTaskCtrlData)
{
  EMaCIError result = KMaCIError;
  
  if (iServerActive) {
    if (aTaskCtrlData.GetTaskCtrlStatusEvent()) {
      // Update sequence.
      ++iTaskCtrlSequenceNumber;
      
      // Got
      SendInterfaceSubscriberMessage(aTaskCtrlData.GetBinBagContainerPtr()->GetBuffer(),
                                     aTaskCtrlData.GetBinBagContainerPtr()->GetBufferSize(),
                                     iTaskCtrlSequenceNumber);

      dPrint(10,"SendInterfaceSubscriberMessage called: %d:%d, seq: %d (%d bytes of payload sent)",
             iInterfaceMajor, iInterfaceMinor, 
             iTaskCtrlSequenceNumber-1,
             aTaskCtrlData.GetBinBagContainerPtr()->GetBufferSize());
      // Result
      result = KMaCIOK;
      
    } else {
      dPrint(2,"No 'TaskCtrlStatusEvent' element in provided CTaskCtrlData object. Will not send!");
      result = KMaCIInvalidFormat;

    }
   
  } else {
    dPrint(2,"Unable to send TaskCtrlData - Interface is not active! (Open() not called?)");
    result = KMaCINotActive;

  }

  return result;
}
//*****************************************************************************

MaCI::EMaCIError CTaskCtrlServer::SendReply(const CTaskCtrlData &aData, 
                                            const SMessageSource &aSource)
{
  EMaCIError result = KMaCIError;


  // Check that server is active.
  if (iServerActive) {

    // Check that compulsory element is found. (This is ofcourse interface specific)
    if (aData.GetTaskCtrlStatusEvent()||aData.GetCommand()) {

      // Send

      result = SendSynchronousMessageReply(aData.GetBinBagContainerPtr()->GetBuffer(),
                                           aData.GetBinBagContainerPtr()->GetBufferSize(),
                                           aSource);
    
    } else {
      dPrint(2,"No 'TaskCtrlEvent or Command' element in provided CTaskCtrlData object. Will not send!");
      result = KMaCIInvalidFormat;
      
    }
    
  } else {
    dPrint(2,"Unable to send TaskCtrlData - Interface is not active! (Open() not called?)");
    result = KMaCINotActive;
    
  }

  return result;
}
//*****************************************************************************

MaCI::EMaCIError CTaskCtrlServer::WaitCommand(CTaskCtrlData &aData, 
                                              SMessageSource &aSource,
                                              const unsigned int aTimeout_ms,
                                              bool aSkipQueued)
{
  dPrint(1,"Wait command called");
  EMaCIError result = KMaCIError;
  const unsigned int begin = ownTime_get_ms();
  int timeout = aTimeout_ms;
  aData.Reset();
  Lock();
  do {
    const unsigned int asize = iCommandArray.size();
    // Check whether there are commands in buffer.
    dPrint(1,"size %d",asize);
    if (asize > 0) {
      // Got element. So; take reference of its pointer, copy it to
      // 'data', delete it. Pop the element from queue.
      
      // Check whether we want to skip queue 
      if (aSkipQueued && asize > 1) {
        // This is only executed if there is >1 in queue.  Otherwise
        // default branch is executed.  Want to skip, so get back()
        // reference. 
        aData= iCommandArray.back().first;
        aSource=iCommandArray.back().second;
        // Remove all elements.
        iCommandArray.clear();
        // print debug
        dPrint(4,"Flushed %d unprocessed commands out of buffer",
               asize-1);

      } else {
        dPrint(1,"No skipping. Copy data");
        // No skipping, so get front() reference;
        aData= iCommandArray.front().first;
        aSource=iCommandArray.front().second;
        iCommandArray.pop_front();
      }
      // Set result.
      result = KMaCIOK;
      
      // Break the loop, skipping Wait()
      break;
    }
    // Wait signal unconditionally.
    if (timeout > 0) Wait(timeout);
 
    // Recalculate timeout
    timeout = aTimeout_ms - ownTime_get_ms_since(begin);
  } while(timeout > 0 && iServerActive);
  Unlock();

  // Change result type if timeout was the reason.
  if (timeout <= 0 && result == KMaCIError) result = KMaCITimeout;
  

  return result;
}
//*****************************************************************************
/*MaCI::EMaCIError CTaskCtrlServer::WaitCommand(CTaskCtrlData &aData, 
                                              SMessageSource &aSource,
                                              const unsigned int aTimeout_ms,
{
  // Call ReceiveInterfaceMessage
  EMaCIError e = ReceiveInterfaceMessage(aData, aSource, aTimeout_ms);

  // Check result
  if (e == KMaCIOK) {
    if (aData.GetCommand() == NULL) {
       dPrint(ODVINFO, "No command in container");
       return KMaCIInvalidFormat;
    }
      

    // If OK is returned, a message was received and succesfully
    // converted to type CTaskCtrlData. Basicly we don't need to do
    // anything here, but some interface may want to do some checks:
    // for example, the received message contains some specific
    // element or information before it is accepted.
  }

  return e;
  }*/
//*****************************************************************************


//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

int CTaskCtrlServer::ProcessMessage(const CTaskCtrlData &aMessage, const SMessageSource &aSource)
{
  int result = 0;
  gim::binbag::CBinBag bb;
  CTaskCtrlData id;

  dPrint(ODTEST,"Message received");



  const TCommand *cmd = aMessage.GetCommand();
  if (cmd) {
    dPrint(ODTEST,"Got command: '%d'", cmd->cmd);
    Lock();
    iCommandArray.push_back(std::make_pair (aMessage,aSource) );
    Notify();
    Unlock();

  }else{

    dPrint(1,"No command in taskctrl data packet...");
  }

  return result;
}
//*****************************************************************************

int CTaskCtrlServer::ThreadFunction(const int aThreadNumber)
{
  int result = 0;
  const unsigned int hard_error_count_limit = 10;
  unsigned int hard_error_count = 0;
  EMaCIError e;
  CTaskCtrlData dd;
  SMessageSource ms;

  assert(aThreadNumber == 0);
  assert(iGIMIPtr != NULL);
  
  dPrint(ODTEST, "GIMIMessage handler started");

  // Run as long as the thread should be active
  while(IsActive()) {
    //dd.Reset();
    // Call the ReceiveMessage wrapper with 1s timeout. This template
    // function model is able to convert to correct type
    // automatically.
    e = ReceiveInterfaceMessage(dd, ms, 2000);
    if (e == KMaCIOK) {
      ProcessMessage(dd, ms);
      
      // Reset error counter.
      if (hard_error_count) hard_error_count = 0;
      
    } else if (e != KMaCITimeout) {
      // Increment hard error count
      ++hard_error_count;
      
      // Check whether the hard error count limit is still under limit. 
      if (hard_error_count < hard_error_count_limit) {
        // Other error, try to recover by sleeping one second.
        dPrint(1,"ProcessMessage returned error %u, trying to recover by sleeping 1000ms (%u attempts left)",
               e, hard_error_count_limit-hard_error_count);
        ownSleep_ms(1000);

      } else {
        dPrint(3,"Error Recovery failed, TaskCtrlServer ThreadFunction terminating...");

        // Break the while().
        break;
      }
      
    }
  }

  dPrint(8, "GIMIMessage handler stopped");
  SetActive(false);

  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

